#!/usr/bin/python
# accessory.py
# License GPLv2
# (c) FrogWorks

import usb.core
import usb.util
import fcntl
import struct
import random
import time
import threading
import os
import sys
import socket

from attribs import *

import time
import RPi.GPIO as GPIO
from Adafruit_ADS1x15 import ADS1x15


ACCESSORY_VID = 0x18D1
ACCESSORY_PID = (0x2D00, 0x2D01, 0x2D04, 0x2D05)

NETLINK_KOBJECT_UEVENT = 15

#load necessary modules
os.system('sudo modprobe w1-gpio')  #one-wire temperature sensor
os.system('sudo modprobe w1-therm') #one-wire temperature sensor
os.system('sudo modprobe i2c-dev')  #i2c for ADC



#*************************************************************************#
#  Initialise sensors and constants
#*************************************************************************#

# file names
lognum_file = '/home/pi/Desktop/pytests/lognum.txt'
log_stop = '/home/pi/Desktop/pytests/logstop.txt'

# initialise temperature sensors
base_dir = '/sys/bus/w1/devices/'
#temp_sensor = ['28-0000041890f3', '28-00000400c851']
temp_sensor = ['28-0000041890f3', '28-00000400c851', '28-000003499455','28-0000041898a7']

# initialise 16-bit ADC. For 12-bit ADC, use ic=0x00
adc = ADS1x15(ic=0x01) 

# adc constants (experimentally determined)
VOLT_MULTIPLIER = 59.75  
CURR_MULTIPLIER = 10000.0

speed_interval = 1.0

# initialise GPIO for hall sensor
GPIO.setmode(GPIO.BOARD) #GPIO.BCM is for processor numbers
GPIO.setup(15, GPIO.IN)
# GPIO.setup(channel, GPIO.OUT, initial=GPIO.HIGH)

# speed tracker and measured constants
speed_log = []
MAGNETS = 4.0
WHEEL_CIR = 1.773

#data stores
hall_stream = [];

#inputs
hall_input = 15

NUM_TEMP = 2
HALL_LOG = True
ADC_LOG = True

#output defaults
DEFAULT_TEMP = 0.0
DEFAULT_VOLT = 0.0
DEFAULT_CURR = 0.00
DEFAULT_SPEED = 0.0

#conversion
METRES_IN_KM = 1000
MINS_IN_HR = 60
SECS_IN_MIN = 60
MV_IN_VOLT = 1000

#precision
PRECISION_SPEED = 2
PRECISION_VOLT = 2
PRECISION_CURR = 2
PRECISION_TEMP = 1

MULTIPLIER_SPEED = 10*PRECISION_SPEED
MULTIPLIER_VOLT = 10*PRECISION_VOLT
MULTIPLIER_CURR = 10*PRECISION_CURR
MULTIPLIER_TEMP = 10*PRECISION_TEMP

SPEED_AVERAGE_WINDOW = 5

lock = threading.Lock();

battery_capacity = 0
capacityFile = "capacityFile.txt"


if __name__ == "__main__":
    pid_file = '/root/program.pid'
    fp = open(pid_file, 'w')
    try:
        fcntl.lockf(fp, fcntl.LOCK_EX | fcntl.LOCK_NB)
    except IOError:
        sys.exit(0)

    main()


def main():

    if len(sys.argv) >= 2:
        parts = sys.argv[1].split('/')
        accessory_task(int(parts[0],16))
        return	

    while True:
        print("no arguments given, starting in daemon mode")
        try:
            sock = socket.socket(
                socket.AF_NETLINK,
                socket.SOCK_RAW,
                NETLINK_KOBJECT_UEVENT
            )

            sock.bind((os.getpid(), -1))
            vid = 0
            while True:
                data = sock.recv(512)
                try:
                    vid = parse_uevent(data)
                    if vid != None:
                        break
                except ValueError:
                    print("unable to parse uevent")

            sock.close()
            accessory_task(vid)
        except ValueError as e:
            print(e)

        print("accessory task finished")


def parse_uevent(data):
        lines = data.split('\0')
        keys = []
        for line in lines:
            val = line.split('=')
            if len(val) == 2:
                keys.append((val[0], val[1]))

        attributes = dict(keys)
        if 'ACTION' in attributes and 'PRODUCT' in attributes:
            if attributes['ACTION'] == 'add':
                parts = attributes['PRODUCT'].split('/')
                return int(parts[0], 16)

        return None

def accessory_task(vid):
    dev = usb.core.find(idVendor=vid)

    if dev is None:
        raise ValueError("No compatible device not found")

    print("compatible device found")

    if dev.idProduct in ACCESSORY_PID:
        print("device is in accessory mode")
    else:
        print("device is not in accessory mode yet,  VID %04X" % vid)

        accessory(dev)

        dev = usb.core.find(idVendor=ACCESSORY_VID)

        if dev is None:
            raise ValueError("No compatible device not found")

        if dev.idProduct in ACCESSORY_PID:
            print("device is in accessory mode")
        else:
            raise ValueError("")
    
    tries = 5
    while True:
        try:
            if tries <= 0:
                break

            dev.set_configuration()
            break
        except:
            print("unable to set configuration, retrying")
            tries -= 1
            time.sleep(1)

    # even if the Android device is already in accessory mode
    # setting the configuration will result in the
    # UsbManager starting an "accessory connected" intent
    # and hence a small delay is required before communication
    # works properly
    time.sleep(1)

    dev = usb.core.find(idVendor = ACCESSORY_VID);

    if dev is None:
        dev = usb.core.find(idVendor = vid);

    if dev is None:
        raise ValueError("device set to accessory mode but VID %04X not found" % vid)

    cfg = dev.get_active_configuration()
    if_num = cfg[(0,0)].bInterfaceNumber
    intf = usb.util.find_descriptor(cfg, bInterfaceNumber = if_num)

    ep_out = usb.util.find_descriptor(
        intf,
        custom_match = \
        lambda e: \
            usb.util.endpoint_direction(e.bEndpointAddress) == \
            usb.util.ENDPOINT_OUT
    )

    ep_in = usb.util.find_descriptor(
        intf,
        custom_match = \
        lambda e: \
            usb.util.endpoint_direction(e.bEndpointAddress) == \
            usb.util.ENDPOINT_IN
    )

    arr = bytearray(8)
    struct.pack_into(">i",arr,0,get_capacity())


    hall_thread = threading.Thread(target = get_hall);
    hall_thread.start();


    writer_thread = threading.Thread(target = writer, args = (ep_out, ))
    writer_thread.start()

    length = -1
    while True:
        try:
            data = ep_in.read(size = 1, timeout = 0)
            print("read value %d" % data[0])
        except usb.core.USBError as e:
            print("failed to send IN transfer")
            print(e)
            break

    writer_thread.join()
    hall_thread.join()

    write_capacity()

    print("exiting application")

def get_capacity():
    global battery_capacity
    f = open(capacityFile,'r')
    battery_capacity = int(f.readlines()[0])
    f.close()

def write_capacity():
    f = open(capacityFile,'w')
    f.write(str(battery_capacity)+"\n")
    f.close()

def accessory(dev):
    version = dev.ctrl_transfer(
                usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_IN,
                51, 0, 0, 2)

    print("version is: %d" % struct.unpack('<H',version))

    assert dev.ctrl_transfer(
            usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_OUT,
            52, 0, 0, MANUFACTURER) == len(MANUFACTURER)

    assert dev.ctrl_transfer(
            usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_OUT,
            52, 0, 1, MODEL_NAME) == len(MODEL_NAME)


    assert dev.ctrl_transfer(
            usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_OUT,
            52, 0, 2, DESCRIPTION) == len(DESCRIPTION)

    assert dev.ctrl_transfer(
            usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_OUT,
            52, 0, 3, VERSION) == len(VERSION)

    assert dev.ctrl_transfer(
            usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_OUT,
            52, 0, 4, URL) == len(URL)

    assert dev.ctrl_transfer(
            usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_OUT,
            52, 0, 5, SERIAL_NUMBER) == len(SERIAL_NUMBER)

    dev.ctrl_transfer(
            usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_OUT,
            53, 0, 0, None)

    time.sleep(1)

def writer (ep_out):
    while True:
        try:
            length = ep_out.write(getValues(), timeout = 0)
            print("%d bytes written" % length)
            time.sleep(0.5)
        except usb.core.USBError:
            print("error in writer thread")
            break

def get_hall():
    while(true):
        hall_pair = [];
        hall_pair.append(time.clock());
        hall_pair.append(GPIO.input(hall_input))
        lock.acquire()
        hall_stream.append(hall_pair)
        lock.release()

def getValues():
    speed = (int)(getSpeed()*100)
    current = (int)(getCurr()*100)
    volt = (int)(getVolt()*100)
    motor = (int)(getMotor()*10)
    ambient = (int)(getAmbient()*10)
    controller = (int)(getController()*10)
    battery = (int)(getBattery()*10)
    arr = bytearray(1024)
    struct.pack_into(">iiiiiii",arr,0,speed,volt,current,motor,ambient,battery,controller)
    return arr



def getSpeed():
    if(HALL_LOG):
        try:
            return round(calc_hall_speed(hall_count_edges(speed_interval), speed_interval), PRECISION_SPEED)
        except:
            return DEFAULT_SPEED
    else:
        return DEFAULT_SPEED

def getCurr ():
    if(ADC_LOG):
        try:
            return round((adc.readADCDifferential23(4096, 250)/MV_IN_VOLT)*CURR_MULTIPLIER, PRECISION_CURR)
        except:
            return DEFAULT_CURR
    else:
        return DEFAULT_CURR

def getVolt ():
    if(ADC_LOG):
        try:
            return round((adc.readADCDifferential01(2048, 250)/MV_IN_VOLT*VOLT_MULTIPLIER), PRECISION_VOLT)
        except:
            return DEFAULT_VOLT
    else:
        return DEFAULT_VOLT

def getMotor ():
    return read_temp(0)

def getAmbient ():
    return read_temp(1)

def getController ():
    return read_temp(2)

def getBattery ():
    return read_temp(3)

def read_temp(num):
    if(num <= NUM_TEMP):
        lines = read_temp_raw(num)
        #temperature numbers start from 1
        
        #checks if sensor is accessible
        while lines[0].strip()[-3:] != 'YES':
            time.sleep(0.2)
            lines = read_temp_raw(num)
            
        #if accessible, look for temp value
        equals_pos = lines[1].find('t=')
        if equals_pos != -1:
            temp_string = lines[1][equals_pos+2:]
            temp_c = float(temp_string) / 1000.0
            return round(temp_c,PRECISION_TEMP)
        else:
            return DEFAULT_TEMP
    else:
        return DEFAULT_TEMP

def read_temp_raw(num):
    device_file = base_dir + temp_sensor[num-1] + '/w1_slave'
    f = open(device_file, 'r')
    lines = f.readlines()
    f.close()
    return lines

def hall_count_edges(interval):
    left_edge = 0
    count = 0
    high = False
    if (len(hall_stream)>1):
        curr_time = time.clock()
        lock.acquire()
        global hall_stream
        for x in range(0,len(hall_stream)):
            if (curr_time - hall_stream[x][0] < interval):
                left_edge = x
                break
        if left_edge == 0: #less than a second of reading
            return 0
        hall_stream = hall_stream[left_edge:]
        for x in range(0,len(hall_stream)-1):
            if (hall_stream[x][1] == 1 and hall_stream[x+1][1] == 1 and high == False):
                high = True
                count += 1
            else if (hall_stream[x][1]==0 and hall_stream[x+1][1]==0 and high == True):
                high = False
                count += 1
        lock.release()
        return count
    else:
        return 0

def calc_hall_speed(count, interval):
    # based on counted edges, return a km/h speed value
    global speed_log
    
    speed = ((count/MAGNETS) * (WHEEL_CIR/interval))/METRES_IN_KM * SECS_IN_MIN * MINS_IN_HR
    
    # append speed, pop first element if speed_log is longer than 5
    speed_log.append(speed)
    if len(speed_log) > SPEED_AVERAGE_WINDOW:
        speed_log.pop(0)
    
    # average across last 5 speeds
    sum = 0.0
    for i in range(len(speed_log)):
        sum += speed_log[i]
        
    return sum/len(speed_log)
